
#include "pid.h"
#include "stdint.h"
#include "stdio.h"
#include "avr/pgmspace.h"

static int16_t Input, Setpoint;
static int32_t Output;

static int16_t outMin, outMax;

uint16_t Compute() {
    Input = (int16_t)PV * 2;
    Setpoint = (int16_t)SV * 2;
    /*Compute all the working error variables*/
    error = (Setpoint - Input) * SCALING_FACTOR;
    ITerm += ki * error;
    //ITerm = 0;
    if (ITerm > outMax) ITerm = outMax;
    else if (ITerm < outMin) ITerm = outMin;
    dInput = (Input - lastInput) * SCALING_FACTOR;

    /*Compute PID Output*/
    Output = kp * error + ITerm - kd * dInput;
    printf_P(PSTR("Output:%d Iterm:%d\n"),Output,ITerm);
    if (Output > outMax) Output = outMax;
    else if (Output < outMin) Output = outMin;
    /*Remember some variables for next time*/
    lastInput = Input;
    return (Output / SCALING_FACTOR);
}
void getTunings(uint8_t * Kp, uint8_t * Ki, uint8_t * Kd){
    
}
void SetTunings(uint8_t Kp, uint8_t Ki, uint8_t Kd) {
    kp = Kp;
    ki = Ki ;
    kd = Kd ;
}

void SetOutputLimits(int16_t Min, int16_t Max) {
    if (Min > Max) return;
    outMin = Min * SCALING_FACTOR;
    outMax = Max * SCALING_FACTOR;

    if (Output > outMax) Output = outMax;
    else if (Output < outMin) Output = outMin;

    if (ITerm > outMax) ITerm = outMax;
    else if (ITerm < outMin) ITerm = outMin;
}

void Initialize() {
    lastInput = PV;
    ITerm = Output;
    if (ITerm > outMax) ITerm = outMax;
    else if (ITerm < outMin) ITerm = outMin;
}